#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <stdlib.h>

int main(int argc, char** argv)
{
    ros::init(argc, argv, "teleop_diffbot");
    ros::NodeHandle nh;

    ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("/diffbot_velocity_controller/cmd_vel", 1000);

    ros::Rate rate(10);

    while (ros::ok())
    {
        geometry_msgs::Twist msg;

        // 这里你可以设置线速度和角速度
        msg.linear.x = 0.2;  // 线速度
        msg.angular.z = 0.2; // 角速度

        pub.publish(msg);

        // ROS_INFO_STREAM("Sending velocity command: " << "linear=" << msg.linear.x << " angular=" << msg.angular.z);

        rate.sleep();
    }

    return 0;
}
